package com.googlecode.grs.controller;

import java.awt.Color;
import java.awt.Graphics;
import java.awt.Point;
import java.awt.event.MouseEvent;
import java.awt.event.MouseListener;
import java.awt.event.MouseMotionListener;

import com.googlecode.grs.core.controller.EventController;
import com.googlecode.grs.robot.SimpleEventMoveRobot;
import com.googlecode.grs.ui.Drawable;
import com.googlecode.grs.ui.SimulatorUI;

/**
 * MouseController allows control of the robot based on the cursor position
 * Driving is enabled and disabled based on left/right mouse clicks.
 * 
 * @author ajc
 * 
 */
public class MouseController extends EventController implements MouseListener,
		MouseMotionListener, Drawable {

	private final SimulatorUI sim;
	private final SimpleEventMoveRobot robot;
	private int x, y;

	private boolean takeAction = false;
	private boolean running = false;

	public MouseController(SimulatorUI sim, SimpleEventMoveRobot robot) {
		this.sim = sim;
		this.robot = robot;
	}

	@Override
	public void startListening() {
		sim.addMouseListener(this);
		sim.addMouseMotionListener(this);
		sim.addDrawable(this);
	}

	@Override
	public void stopListening() {
		sim.removeMouseListener(this);

	}

	// move towards point
	public void run() {
		running = true;
		while (running) {
			if (takeAction) {
				if (!isRobotHeadingToMouse()) {
					/*
					 * Correction doesn't find the fastest path when turning
					 * across the 180deg line.
					 */
					if (shouldCorrectToRight()) {
						turnRight();
					} else {
						turnLeft();
					}
				} else if (!isPositionAtCursor()) {
					driveForward();
				}

			}
			// log("" + Math.toDegrees(getMouseHeading()));

			try {
				sleep(20);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
	}

	/**
	 * MouseHeading is the robot 'mouseHeading', robot 'heading' relative to a
	 * 'mouse compass'
	 * 
	 * If the robot is left of the cursor's heading, then this value is
	 * negative.
	 * 
	 * @return
	 */
	private double getMouseHeading() {
		return robot.getHeading() - angleToMouse();
	}

	/**
	 * Gets the angle from robot to the cursor, relative to heading
	 * 
	 * @return
	 */
	private double angleToMouse() {
		double deltaY = y - robot.getY();
		double deltaX = x - robot.getX();
		return Math.atan2(deltaY, deltaX);
	}

	/**
	 * 
	 * @return
	 */
	private boolean isRobotHeadingToMouse() {
		final double ANGLE_TOLERANCE = Math.PI / 64;
		return Math.abs(getMouseHeading()) < ANGLE_TOLERANCE;
	}

	/**
	 * Turns the robot slightly left Used when the robot's heading is greater
	 * than that of the mouse
	 */
	private void turnLeft() {
		final int TURN_STEP = 2;
		robot.turnLeft();
		robot.waitAngle(TURN_STEP);
	}

	/**
	 * Turns the robot slightly left. Used when the robot's heading is less than
	 * that of the mouse
	 */
	private void turnRight() {
		final int TURN_STEP = 2;
		robot.turnRight();
		robot.waitAngle(TURN_STEP);
	}

	/**
	 * Checks if the robot should correct its heading by turning right
	 * 
	 * @return true if robot heading is less than direction to mouse
	 */
	private boolean shouldCorrectToRight() {
		return getMouseHeading() < 0;
	}

	/**
	 * 
	 * @return
	 */
	private boolean isPositionAtCursor() {
		// Point2D.Double cursor = new Point2D.Double(x,y);
		final double DISTANCE_TOLERANCE = 2;
		return Point.distance(x, y, robot.getX(), robot.getY()) < DISTANCE_TOLERANCE;
	}

	/**
	 * 
	 */
	private void driveForward() {
		final int DRIVE_STEP = 1;
		robot.goForward();
		robot.waitDistance(DRIVE_STEP);
	}

	@Override
	public void mouseClicked(MouseEvent arg0) {
		// TODO Auto-generated method stub

	}

	@Override
	public void mouseEntered(MouseEvent arg0) {
		// TODO Auto-generated method stub

	}

	@Override
	public void mouseExited(MouseEvent arg0) {

	}

	/**
	 * Start moving to the cursor's position
	 */
	public void mousePressed(MouseEvent arg0) {
		// if( arg0.getButton() == MouseEvent.BUTTON1) {
		//
		// }
		takeAction = arg0.getButton() == MouseEvent.BUTTON1;
	}

	/**
	 * Stop moving to the the cursor's position
	 */
	public void mouseReleased(MouseEvent arg0) {
		// takeAction = false;
	}

	public void mouseDragged(MouseEvent arg0) {
		// arg0.
		// not used
	}

	public void mouseMoved(MouseEvent arg0) {

		x = arg0.getX();
		y = arg0.getY();
		// log("new point:" + x + ", " + y);
		// TODO Auto-generated method stub

	}

	public void draw(Graphics g) {
		g.setColor(Color.RED);
		g.fillRect(x - 5, y - 5, 10, 10);
	}

	@Override
	public void halt() {
		// TODO Auto-generated method stub

	}

	@Override
	public void pause() {
		// TODO Auto-generated method stub

	}

	@Override
	public void unPause() {
		// TODO Auto-generated method stub

	}

}
